
#include <msp430x26x.h>
//#include "UART_HANDLER.h"
#include "PortDef.h"
#include <stdio.h>

/******************************************************
 * VERSION NUMBER - manually update this as substantive changes are made
 ******************************************************/
const char *VER_NUM = "2.0 C";   // this is the version number
/******************************************************/

/******************************************************
 * variable declarations
 ******************************************************/
//signed int PIDControl(volatile long, static int, static signed int, static signed int, static signed int, static int);
 //Motor Control Variables
 volatile long prevServ1PWMIn=0;
 volatile long prevServ2PWMIn=0;
 volatile long prevServ3PWMIn=0;
 volatile long prevServ4PWMIn=0;
 volatile long newServ1PWMIn;
 volatile long newServ2PWMIn;
 volatile long newServ3PWMIn;
 volatile long newServ4PWMIn;
 volatile int  Serv1InPos=0;
 volatile int  Serv2InPos=0;
 volatile int  Serv3InPos=0;
 volatile int  Serv4InPos=0;
 volatile long temp1;
 volatile long temp2;
 volatile long temp3;
 volatile long temp4;
 volatile long varMotorOut1;
 volatile long varMotorOut2;
 volatile long InitServoOut1;
 volatile long InitServoOut2;
 volatile signed long throttle;
 volatile long raw_yaw;
 volatile long right_yaw;
 volatile long left_yaw;
 volatile long motor1_yaw;
 volatile long motor2_yaw;
 //Magnetometer Variables
 volatile int magXaxisRead = 6;
 volatile long max_A = 0xffff;
 //Sonar Variables
 volatile unsigned int sonarCount = 0;
 volatile long tempSonar = 0;
 volatile long meanSonar = 0;
 volatile long sonarResult;
 static int targetValue = 500;                          //based on sonar calibration, this is 24 inches
 long sonarVal;
 //Timer Variables
 volatile unsigned int i;
 volatile unsigned long TB_CtrProxy = 0;
 volatile unsigned long prevTB_CtrProxy = 0;
 volatile unsigned long max_TB_CtrProxy = 39999;
 volatile unsigned int count=0;
 volatile unsigned int servCount = 0;
 volatile unsigned int twenty_ms_tick = 0;
 volatile unsigned int ten_ms_tick = 0;
 int Kp = 2;                                            //proportional gain
 int Kd = 2;
 signed int currentError;                                   //current difference between targetValue and meanSonar for PID control
 signed int prevError = 0;                              	//previous error for PID control
 signed int PIDout=0;
 volatile int PID_tick = 0;                             	//sets flag to run PID equation

/******************************************************
 * INTERRUPT DECLARES - declare ISRs and link their handlers
 ******************************************************/
/*__interrupt void usart1_rx(void);
USCIAB1RX_ISR(usart1_rx)*/

__interrupt void TB_overflow(void);
TIMERB0_ISR(TB_overflow)

__interrupt void servoPWM_in(void);
TIMERB1_ISR(servoPWM_in)

//__interrupt void spi_mag_tx(void);
//USCIAB0TX_ISR(spi_mag_tx)

//__interrupt void spi_mag_rx(void);
//USCIAB0RX_ISR(spi_mag_rx)

__interrupt void adc_sonar(void);
ADC12_ISR(adc_sonar)
/******************************************************/

/******************************************************
Variable Declarations, initializations
******************************************************/
//struct UART_RX_BUF S_UART_BUFFER; //declare the buffer object to be used
//unsigned char UART_ON,MENU_ON,CHANNELSET_ON;
/*****************************************************/

/*****************************************************
Function declarations
*****************************************************/
void ServMotCtl(void);
/****************************************************/

/******************************************************
 * InitSys - initialize and configure system after reset
 ******************************************************/
void InitSys(void)
{
// *********************** CLOCKS ****************************************
     //XT1 has a 32.768kHz crystal attached
     //XT2 has a 16MHz crystal attached
     int i;
     WDTCTL = WDTPW + WDTHOLD;     	 		 // Stop WDT, then proceed to dream about it (Jeff)
     _disable_interrupts();          		 // DINT
     _bic_SR_register(OSCOFF);       		 // Turn on oscillators
     BCSCTL1 = 0x00;                         // XT2 ON, LFXT1 = low freq, ACLK div = 1
     BCSCTL3 = (XT2S_2+LFXT1S_0+XCAP_3); 	 //XT2 uses 16MHz crystal, XT1 in low freq op, XT1 uses 12.5pf cap,
     do
             {
                     IFG1 &= ~(OFIFG);                               // Clear OFIFG
                     for(i = 0; i<255;i++) _nop();   				 // Delay
             }
     while(IFG1&OFIFG);                      		 // Loop while OFIFG bit in IFG1 = 1
     BCSCTL2 = (SELM_2+DIVM_0+SELS+DIVS_1);          // MCLK=XT2, div=1, SMCLK=XT2, div = 2
     //so, at this point the ACLK=32768Hz, MCLK=16MHz, SMCLK=8MHz, DCO is off

// *********************** PORTS *****************************************

     //setup outputs (low by default)
     P1OUT = 0;
     P2OUT = (0+SPIEnblGyroZ+SPIEnblAccel+SPIEnblPres);      // SPI enables are active low, so deactivate by setting high
     P3OUT = (0+SPIEnblMag);
     P4OUT = 0;
     P5OUT = 0;
     P6OUT = 0;

//  I/O configured as outputs by default to eliminate floating inputs.
     P1DIR = 0xFF;
     P2DIR = (0xFF & ~MagDatRdy & ~PresDatRdy);      						   //MagDatRdy, PresDatRdy are inputs
     P3DIR = (0xFF & ~SPIMiSo);      										   //MiSo is an input
     P4DIR = (0xFF & ~Serv1PWMIn & ~Serv2PWMIn & ~Serv3PWMIn & ~Serv4PWMIn);   //Serv(1-4)PWMIn are inputs
     P5DIR = 0xFF;   														   // All P5.x outputs
     P6DIR = 0xFF;   														   // All P6.x outputs
// Set up pin special functions.
      P1SEL = (Serv1PWMOut+Serv2PWMOut);  									   //  PWM outputs from Timer
      P3SEL = (UART_TxToGPS+UART_RxFromGPS);      							   //connect to UARTs
      P4SEL = (Mot1PWMOut+Mot2PWMOut+Serv1PWMIn+Serv2PWMIn+Serv3PWMIn+Serv4PWMIn);    //connect to timer module
      P6SEL = (Son1In+Son2In+Son3In+Son4In+GyroX+GyroY+GyroXYRef);    		   //connect to ADC
// *********************** UART ******************************************
     //configure UCA1 (for GPS or debug terminal)
     /*UCA1CTL0 = 0;                                        // 8N1 UART config
     UCA1CTL1 |= (UCSSEL_2+UCSWRST);     					// SMCLK source and hold UART in reset
     UCA1BR0 = 13;                           				// see tables in user guide-38400bps
     UCA1BR1 = 0;                            
     UCA1MCTL = UCOS16;                              		// oversampling mode
     UCA1CTL1 &= ~UCSWRST;                   				// **Initialize USCI state machine**
     UC1IE |= UCA1RXIE;                  					// Enable USCI_A1 RX interrupt
 // Initialize the RX buffer structure.
     for(i=0;i<60;i++) S_UART_BUFFER.Buffer[i]=0;
     S_UART_BUFFER.Buf_Strt = &S_UART_BUFFER.Buffer[0];     //get the address of the buffer head
     S_UART_BUFFER.Buf_End = &S_UART_BUFFER.Buffer[0];      //get the address of the buffer tail
     S_UART_BUFFER.Buffer_Length = 0;                       //initialize the buffer length to 0

     UART_ON = 1;                                           // variable to indicate that the UART is open to programming
     MENU_ON = 0;

     tx_byte_to_slave(0x0D);
     tx_byte_to_slave(0x0A);
     tx_slave_string("IMUboard restart");
     tx_byte_to_slave(0x0D);
     tx_byte_to_slave(0x0A);*/


//*******************USCI: SPI Mode*******
/*UCB0CTL1 |= (UCSSEL_2 + UCSWRST); 					  //SCLK, set UCSWRST
UCB0CTL0 |= (UCCKPH + UCMSB + UCMST + UCMODE_0 + UCSYNC); //data captured on first UCLK egde, polarity = inactive low, MSB first, 8-bit data, master mode, USCI mode ?, synchronous mode
UCB0CTL1 &= ~UCSWRST;  									  //untoggle software reset
IE2 = UCB0RXIE;   										  //enable receive interrupts
UCB0TXBUF = 0x42;*/              						  //see Magnetometer data sheet, /512, read x-axis

// ************************ Timer A  ******continuous mode, timer overflow event to make the timer reset, CCR0 as another interrupt at 20ms, when you get over 65,000and change, subtract, set to run every 20ms set new value to CCR0
//TACCTL0 = (CCIE);
TACCTL1 = (OUTMOD_7);               //CCR1 set mode
TACCTL2 = (OUTMOD_7);               //CCR2 set mode
TACCR0 = 39999;                     //timer period =20ms, which is 40000-1 clock cycles (since 0 gets counted)
TACCR1 = InitServoOut1;
TACCR2 = InitServoOut2;
TACTL = (TASSEL_2 + ID_2+ MC_1);    //SMCLK, /4, up mode, interrupts enabled, compare mode

// ************************ Timer B ******
TBCCTL0 = CCIE;									//enable timer B interrupts
TBCCTL1 = (OUTMOD_7);                           //CCR1 toggle/reset
TBCCTL2 = (OUTMOD_7);                           //CCR2 toggle/reset
TBCCTL3 = (CCIS_0 + CM_3 + CAP + CCIE);         //capture/compare input select, capture on rising and falling edge, capture mode, capture/compare interrupt enabled
TBCCTL4 = (CCIS_0 + CM_3 + CAP + CCIE);         //capture/compare input select, capture on rising and falling edge, capture mode, capture/compare interrupt enabled
TBCCTL5 = (CCIS_0 + CM_3 + CAP + CCIE);         //capture/compare input select, capture on rising and falling edge, capture mode, capture/compare interrupt enabled
TBCCTL6 = (CCIS_0 + CM_3 + CAP + CCIE);         //capture/compare input select, capture on rising and falling edge, capture mode, capture/compare interrupt enabled
TBCCR0 = 500;                                   // PWM period = 250 us, which is 500 clock cycles
TBCCR1 = varMotorOut1;
TBCCR2 = varMotorOut2;
TBCTL = (CNTL_0 + TBSSEL_2 + ID_2+ MC_1);    	//SMCLK, /4, up mode

//****************ADC Sonar******************
ADC12CTL0 = (REFON + ADC12ON + REF2_5V);          	//reference generator on, ADC12 on, 2.5V ref
ADC12CTL1 = (SHS_0 + CONSEQ_0 + SHP + CSTARTADD_0); //conversion memory register 0, single channel single conversion, write to memory register 0
ADC12MCTL0 = (SREF_1 + INCH_0);         			//select reference: VR+ = Vref and VR-- = AVSS, input channel select: A0
ADC12IE = 0x01;                         			//turn on interrupt for register 0
ADC12CTL0 |= ENC;                                   //enable conversion

_enable_interrupts();   //this is the last thing that the Init Routine should do, since we don't want ISR activity before everything is initialized
}

/*********************************************************
main - the main scheduling and code loop
*********************************************************/
void main(void)
{
     InitSys();
     while(1) //main polling loop
     {
             //if(S_UART_BUFFER.Buffer_Length > 0) S_parseRXCmd();     // if receive buffer contains data, decide what to do w/ it
             //main loop - updates every 10 msec
             if(ten_ms_tick==1){
                     ten_ms_tick=0;
                     ADC12CTL0 |= ADC12SC;               //ADC12SC bit low to high in order to sample
                      //NOTE! MOTOR AND SERVO CONTROL UPDATES SHOULD ONLY BE MADE IN THIS LOOP!
                     ServMotCtl();    //command the servos and motors
                     if(PID_tick==1)
                     {
                         sonarVal = meanSonar;
                         currentError = targetValue - meanSonar;
                         if (currentError < 0){
                         	PIDout = 0;
                         }
                         else {PIDout = Kp*currentError + (Kd*(currentError-prevError));} //+ (Ki*(currentError-prevError)*time) ;
                         prevError = currentError;
                         PID_tick = 0;					//Reset PID_tick
                     }

             }
     }
}
/*********************************************************
ServMotCtl() - uses global state variables to command the servo and motor controls
performs mixing from thrust and yaw to motor commands
*********************************************************/
void ServMotCtl(void)
{
   //for now, just loop servo commands back out
   TACCR1 = Serv3InPos;     //ROLL
   TACCR2 = Serv4InPos;     //PITCH
  //Yaw adjustment
  raw_yaw = (Serv2InPos/8 - 274);					//Set yaw value's incoming value go from a range of 0-205.  These numbers came from the receiver output signals
  if (raw_yaw <= 95){
      left_yaw = (95 - raw_yaw)*.5;
      }
  if ((raw_yaw < 105) & (raw_yaw > 95)){			//Set the middle range to not cause any yaw in the system
       left_yaw = 0;
       right_yaw = 0;
       }
  if (raw_yaw >= 105){
      right_yaw = (raw_yaw - 104)*0.5;
      }
   throttle = ((Serv1InPos/8)-250)*2.39234 + PIDout;	//Change range of the throttle to be between 0-500, also take into account PID error
   if (throttle > 500){
       throttle = 500;
   }
   if (throttle < 0){
       throttle = 0;
   }
   motor1_yaw = throttle - left_yaw;
   motor2_yaw = throttle - right_yaw;
  if (motor1_yaw < 0) {								//Make sure there are no negative motor values
               motor1_yaw = 0;
       }
  if (motor2_yaw < 0) {
               motor2_yaw = 0;
       }
   //for motors, need to mix Serv1InPos (thrust) and Serv2InPos (yaw) to get the two motor commands
   TBCCR1 = motor1_yaw;					 //Top rotor
   TBCCR2 = motor2_yaw;                  //Bottom Rotor

}
/********************************************************/

/*********************************************************
ISR: usart0_rx - this code gets executed whet USART0 receives a byte
*********************************************************/
/*__interrupt void usart1_rx(void)
{
     tx_byte_to_slave(UCA1RXBUF); //echo the byte out
     S_UART_BUFFER.Buffer[S_UART_BUFFER.Buffer_Length] = UCA1RXBUF;  // Copy Data
     if(S_UART_BUFFER.Buffer_Length < 60) S_UART_BUFFER.Buffer_Length++; // increment buffer length
     else S_UART_BUFFER.Buffer_Length = 0;  // reset buffer if we would have written past the end
}*/
/***************************************************
 * ISR: TBIV (timer B interrupt vector) servo PWM in
 * ************************************************/
 //interrput signal set by incoming radio signal going high or low
 //not trigger interrupt for caputre in software, but when line changes state, capture occurs on the high/low
 //capture the time that we receive the signal - set the bits in the cc config register to enable int on cap event and set interrupt on cap event
 //look at schematic structure of timer module and see how each flag comes out of CC modules
__interrupt void servoPWM_in(void)
{
     switch (TBIV)
     {
             case 0:
             {
                     break;
             }
             case 2:
             {
                     break;
             }
             case 4:
             {
                     break;
             }
             case 6:    //Servo 1PWMIn - RC Ch3 - Throttle
             {
                     newServ1PWMIn = TB_CtrProxy + TBCCR3;
                     if ((newServ1PWMIn - prevServ1PWMIn) < 0)
                     {
                         temp1 = (max_TB_CtrProxy - prevServ1PWMIn) + newServ1PWMIn;
                     }
                     else temp1 = newServ1PWMIn - prevServ1PWMIn;
                     if ((temp1 > 1600) & (temp1 < 4000)) {   //Check to see if PWM value goes over the bound
                         Serv1InPos = temp1; //if the measurement is within bounds, record it
                     }
                     prevServ1PWMIn = newServ1PWMIn;
               break;
             }
             case 8:    //Servo 2PWMIn - RC Ch4 - Yaw
             {
                     newServ2PWMIn = TB_CtrProxy + TBCCR4;
                     if ((newServ2PWMIn - prevServ2PWMIn) < 0)
                     {
                         temp1 = (max_TB_CtrProxy - prevServ2PWMIn) + newServ2PWMIn;
                     }
                     else temp1 = newServ2PWMIn - prevServ2PWMIn;
                     if ((temp1 > 1600) & (temp1 < 4000)) {
                         Serv2InPos = temp1; //if the measurement is within bounds, record it
                     }
                     prevServ2PWMIn = newServ2PWMIn;
               break;
             }
             case 10:    //Servo 3PWMIn - RC Ch1 - Roll
             {
                   newServ3PWMIn = TB_CtrProxy + TBCCR5;
                      if ((newServ3PWMIn - prevServ3PWMIn) < 0)
                   {
                         temp1 = (max_TB_CtrProxy - prevServ3PWMIn) + newServ3PWMIn;
                   }
                   else temp1 = newServ3PWMIn - prevServ3PWMIn;
                   if ((temp1 > 1600) & (temp1 < 4000))
                   {
                       Serv3InPos = (int)temp1; //if the measurement is within bounds, record it
                   }
                   prevServ3PWMIn = newServ3PWMIn;
               break;
             }
             case 12:    //Servo 4PWMIn - RC 2 - Pitch
             {
                      newServ4PWMIn = TB_CtrProxy + TBCCR6;
                   if ((newServ4PWMIn - prevServ4PWMIn) < 0)
                   {
                       temp1 = (max_TB_CtrProxy - prevServ4PWMIn) + newServ4PWMIn;
                     }
                   else temp1 = newServ4PWMIn - prevServ4PWMIn;
                   if ((temp1 > 1600) & (temp1 < 4000)) {
                       Serv4InPos = (int)temp1; //if the measurement is within bounds, record it
                   }
                   prevServ4PWMIn = newServ4PWMIn;

                     //P1OUT ^= 0x04;//testing
                break;
             }
     }
}
//*****************************************************
//ISR:spi_mag_tx
//transmit data to magnetometer
//*****************************************************
/*__interrupt void spi_mag_tx(void)
{
     UCB0TXBUF = 0x42;  //see Magnetometer data sheet, /512, read x-axis
}*/

//*****************************************************
//ISR: spi_mag_rx
//receive data to magnetometer
//*******************************************************
/*__interrupt void spi_mag_rx(void)
{

      //if(MagDatRdy == 1){
              while(!(IFG2 & UCA0TXIFG));   //USART1 TX buffer ready?
      magXaxisRead = UCB0RXBUF;    			//move input from receive buffer to variable
      //UCA0TXBUF = 0x42;            		//see magnetometer data sheet, /512, read x-axis
      for(i=30;i>0;i--);           			//wait for

      //}
}*/

//*****************************************************
//ISR: sensor_read
//Timer B interrupt, occurs every 250 usec
//*****************************************************
__interrupt void TB_overflow(void)
{
         count = count + 1;
         servCount = servCount + 1;
         if (count == 40){                            //Set up 10 ms counter for sensor read
              ten_ms_tick  = 1;
              count = 0;
         }
         if (servCount == 80){                        //Set up 20 ms counter for servo control
              twenty_ms_tick = 1;
              servCount = 0;
         }
         TB_CtrProxy = TB_CtrProxy + TBCCR0+1;        //update pseudo-timer counter, account for counting 0
         if (TB_CtrProxy > max_TB_CtrProxy){   		  //TB_CtrProxy overflow
              TB_CtrProxy = (TB_CtrProxy - max_TB_CtrProxy);
         }
}

//*****************************************************
//ISR: adc_sonar
//analog to digital conversion for sonar 1
//*****************************************************
__interrupt void adc_sonar(void)
{
     switch (ADC12IV)
     {
             case 6:
             {
                 sonarCount = sonarCount + 1;                  		//Sonar calibration 8.653 per inch + 298, initial value of 6 inches = 330
                 sonarResult = ADC12MEM0;                           //put conversion result into variable
                 tempSonar = tempSonar + sonarResult;  				//add sonar reults to get average
                 if (sonarCount == 10){
                     meanSonar = tempSonar/10;                 		//average sonar results
                     sonarCount = 0;								//reset sonar counter
                     PID_tick = 1;                                  //set tick to perform PID calculation
                     tempSonar = 0;
                       }

                 break;
             }
    }
}
